//#include "HardwareUsart.h"

#include "../../../bit/bit.h"
//#include "HardwareUsartInterface.h"
#include "../../../container/vector/Vector.h"
#include "../../../protocol/Protocol.h"
#include "../../../protocol/ProtocolImplementation.h"
#include "../../../container/queue/Queue.h"
#include "../../../soa/service/Service.h"
#include "../../../soa/soa/Soa.h"

#include<stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>



//#define F_CPU 1000000
//#include <avr/delay.h>


volatile unsigned char usartTransmitFlag=0;
volatile unsigned char usartReceiveFlag=0;
volatile unsigned char usartDataEmptyFlag=0;
volatile unsigned char usartReceivedDataTemp=0;
volatile unsigned char dataIn = 0;
volatile unsigned char dataOut = 0;


class UsartService : public Service
{
	public :
		typedef struct USART_DATA
		{
			unsigned char in;
			unsigned char out;
		} UsartData;
		static Queue<UsartData> inQueue;
		static Queue<UsartData> outQueue;
		static UsartData data;

		UsartService(){
			UsartData d;
			d.in = 0; d.out = 0;
			inQueue.push(d);
			
		}
		UsartService(unsigned char d)
		{
			UsartData d2;
			d2.in = d;
			d2.out = 0;
			inQueue.push(d2);
		}
		void start()
		{
			if(dataQueue.empty() != 1)
				data = (dataQueue.pop());
			UDR = data.in;
		} //set data to transmit
		void run()
		{
			//wait for empty transit buffer and data to be received
			if((UCSRA & (1<<UDRE)) & (UCSRA & (1<<RXC)) )
			{
				finished = true;
			}	
		}
		void stop()
		{
			finished = false;
			data.out = UDR; 
			outQueue.push(data);
		} //get the data out the data received.
};





class Terminal2
{
	public :
		Soa soa;
	UsartService usart;
	Terminal2()
	{
		soa = Soa();
		usart = UsartService();
		soa.registerServices.push_back(&usart);
	}
	void init()
	{
			UCSRB = (1<<RXEN)|(1<<TXEN);
			UCSRC = (1<<URSEL)|(0<<USBS)|(3<<UCSZ0);
			DDRD = SetBit(DDRD, 0, 0); //set receive as input
			DDRD = SetBit(DDRD, 1,1); //set transmit as output
			PORTD = SetBit(PORTD, 0, 1);
			PORTD = SetBit(PORTD, 1, 0);

			//UBRRH = 0x00;
			UBRRL = 0x0C; //should be 4800 for 1 MHz.
			//UCSRB = UCSRB | 0x80; //change 0xC0 to 0x80
			//sei();
			//UDR = '1';
			//unsigned char t = UDR;
			//UDR = '1';
	}
	void usartTransmit(unsigned char b)
	{
		soa.requests.add(0);
		UsartService(b); //set data to transmit.
	}
	unsigned char usartReceive()
	{
		if(soa.currentService == NULL) //do one 
			soa
		//run all requests until finished then do receive request.
		while(soa.request.empty() != 1)
		{
			soa.run();
		}
		//add another request and run until finished.
		UsartService();
		soa.request.add(0);
		
	}
	//this does poll
	void writeByte(unsigned char byte)
	{
		usartTransmit(byte);
	}
	unsigned char readByte()
	{
		return usartReceive();
	}

	void write(unsigned char* data, unsigned long int length)
	{
		unsigned long int i = 0;
		for(i = 0; i< length; i++)
			writeByte(data[i]);
	}
	void read(unsigned char* buffer, unsigned long int length)
	{
		unsigned long int i = 0;
		for(i = 0; i < length; i++)
			buffer[i] = readByte();
	}




};

class Terminal
{
	public :
	Terminal(){}
	void init()
	{
			UCSRB = (1<<RXEN)|(1<<TXEN);
			UCSRC = (1<<URSEL)|(0<<USBS)|(3<<UCSZ0);
			DDRD = SetBit(DDRD, 0, 0); //set receive as input
			DDRD = SetBit(DDRD, 1,1); //set transmit as output
			PORTD = SetBit(PORTD, 0, 1);
			PORTD = SetBit(PORTD, 1, 0);

			//UBRRH = 0x00;
			UBRRL = 0x0C; //should be 4800 for 1 MHz.
			//UCSRB = UCSRB | 0x80; //change 0xC0 to 0x80
			//sei();
			//UDR = '1';
			//unsigned char t = UDR;
			//UDR = '1';
	}
	void usartTransmit(unsigned char b)
	{
		//wait for empty transit buffer
		while(!(UCSRA & (1<<UDRE)));
		UDR = b; //put data into buffer and send
	}
	unsigned char usartReceive()
	{
		//wait for data to be received
		while(!(UCSRA & (1<<RXC)));
		//get udr
		return UDR;
	}
	//this does poll
	void writeByte(unsigned char byte)
	{
		usartTransmit(byte);
	}
	unsigned char readByte()
	{
		return usartReceive();
	}

	void write(unsigned char* data, unsigned long int length)
	{
		unsigned long int i = 0;
		for(i = 0; i< length; i++)
			writeByte(data[i]);
	}
	void read(unsigned char* buffer, unsigned long int length)
	{
		unsigned long int i = 0;
		for(i = 0; i < length; i++)
			buffer[i] = readByte();
	}




};




		void disableTransmitInterrupt(){UCSRB = UCSRB  & ~(0x40);}
		void enableTransmitInterrupt(){ UCSRB = UCSRB | 0x40; }
		void enableReceiveInterrupt(){UCSRB = UCSRB | 0x80; }
		void disableReceiveInterrupt(){UCSRB = UCSRB & ~(0x80);}
//should be started by reading UDR
void startReceiveInterrupt(){ dataOut = UDR;}
//should be started by setting the UDR.
void startTransmitInterrupt(){ UDR = dataIn; dataIn = 0;}
//should be triggered by the receive flag
void triggerTransmitInterrupt(){ UCSRA = UCSRA | (1<<TXC);}
void triggerReceiveInterrupt(){ UCSRA = UCSRA | (1<<RXC);}

void stopReceiveInterrupt(){ UCSRA = UCSRA & ~(1<<RXC);}
void stopTransmitInterrupt(){UCSRA = UCSRA & ~(1<<TXC);}

Terminal terminal = Terminal();
//Terminal2 terminal = Terminal2(&terminal2);
//Queue<unsigned char> value = Queue<unsigned char>();



volatile bool receiveTriggerFlag = false;
volatile bool transmitTriggerFlag = false;
volatile bool receiveCompleteFlag = false;
volatile bool transmitCompleteFlag = false;
volatile bool test = false;




int main()
{
	Queue<unsigned char> happy = Queue<unsigned char>();
	Queue<unsigned char> line = Queue<unsigned char>();
	Vector<unsigned char> line2 = Vector<unsigned char>();
	DDRA = 0xFF;
	DDRB = 0xFF;
	PORTB = 0x00;
	PORTA = 0x00;
	DDRC = 0xFF;
	PORTC = 0x00;
	DDRD = SetBit(DDRD, 0, 0); //set receive as input
	DDRD = SetBit(DDRD, 1,1); //set transmit as output
	PORTD = SetBit(PORTD, 0, 1);
	PORTD = SetBit(PORTD, 1, 0);


	UBRRL = 0x0C; //should be 4800 for 1 MHz.



	terminal.init();
	sei();
	//UDR = '1';
	while(1)
	{

			unsigned char temp = terminal.readByte();
			if(temp == '\b')
			{
				terminal.writeByte('\b');
				terminal.writeByte(' ');
				terminal.writeByte('\b');
				line2.pop_back();
			}
			else if(temp == '\r' || temp == '\n')
			{
				unsigned char test[] = "\n\r";
				terminal.write(test, sizeof(test));
				
				unsigned char t[] = "\n\recho \n\r";
				terminal.write(t, sizeof(t));
				while(line.empty() != 1)
					terminal.writeByte(line.pop());
				
				terminal.write(test, sizeof(test));
				unsigned int i = 0; 
				for(i = 0; i < line2.size(); i++)
					terminal.writeByte(line2[i]);
				terminal.write(test, sizeof(test));
				
			}
			else if(temp == 0 || temp == 0xff)
			{}
			else
			{
				terminal.writeByte(temp);
				line.push(temp);
				line2.push_back(temp);
			}
		
		if(temp == '\n' || temp == '\r')
		{
			unsigned char awesome[] = "command";
			unsigned int i = 0;
			bool check = true;
			for(i = 0; i < sizeof(awesome)-1; i++)
			{
				if(line2[i] != awesome[i])
					check = false;
			}
			if(check == true)
			{
				unsigned char success[] = "SUCCESS !!!!\n\r";
				terminal.write(success, sizeof(success));

			}
			else
			{
				unsigned char success[] = "FAIL !!!!\n\r";
				terminal.write(success, sizeof(success));
			}
			
			while(line2.size() != 0)
				line2.pop_back();
			
		}
	}

}

